
/**
 * @file algo_calculus.c
 * @brief Software implementation of Value calculus for 51 microcontroller.
 * @author David Xu &J. Camilo Gomez C.
 * @version 1.0.0
 * @date 2023-11-21
 * @note none
 */

#include "algo_calculus.h"


#define Cals_Update(x)     \
    (x)->x[2] = (x)->x[1]; \
    (x)->x[1] = s

/*============================================================================*/
void Cals_StateInit(Cals_state_t *x, const int16_t x0, const int16_t sn_1, const int16_t sn_2) {
    x->x[0] = x0;
    x->x[1] = sn_1;
    x->x[2] = sn_2;
}
/*============================================================================*/
int16_t Cals_IntegralRe(Cals_state_t *x, const int16_t s, const int16_t dt) {
    x->x[0] += s * dt;
    Cals_Update(x);

    return x->x[0];
}
/*============================================================================*/
int16_t Cals_IntegralTr(Cals_state_t *x, const int16_t s, const int16_t dt) {
    x->x[0] += ((s + x->x[1]) >> 1) * dt;
    Cals_Update(x);

    return x->x[0];
}
/*============================================================================*/
int16_t Cals_IntegralSi(Cals_state_t *x, const int16_t s, const int16_t dt) {
    x->x[0] += ((s + (x->x[1] << 2) + x->x[2]) * dt) / 6u;
    Cals_Update(x);

    return x->x[0];
}
// Simpson's 3/8  Method:x->x[0] += ((x->x[0] + 3 * (x->x[1] + x->x[2]) + x->x[3]) * dt * 3) / 8
/*============================================================================*/
int16_t Cals_Derivative(Cals_state_t *x, const int16_t s, const int16_t dt) {
    x->x[0] = (s - x->x[1]) / dt;
    Cals_Update(x);

    return x->x[0];
}
/*============================================================================*/



#define Cals_FpUpdate(x)   \
    (x)->x[2] = (x)->x[1]; \
    (x)->x[1] = s

void Cals_FpStateInit(Cals_Fpstate_t *x, const qFP16_t x0, const qFP16_t sn_1, const qFP16_t sn_2) {
    x->x[0] = x0;
    x->x[1] = sn_1;
    x->x[2] = sn_2;
}

qFP16_t Cals_FpIntegralRe(Cals_Fpstate_t *x, const qFP16_t s, const qFP16_t dt) {
    x->x[0] = qFP16_Add(x->x[0], qFP16_Mul(s, dt));
    Cals_FpUpdate(x);

    return x->x[0];
}

qFP16_t Cals_FpIntegralTr(Cals_Fpstate_t *x, const qFP16_t s, const qFP16_t dt) {
    // x->x[0] += ((s + x->x[1]) /2 ) * dt;
    qFP16_t tmp1 = qFP16_Add(s, x->x[1]);
    qFP16_t tmp2 = qFP16_Div(tmp1, qFP16_Constant(2));
    qFP16_t tmp3 = qFP16_Mul(tmp2, dt);
    x->x[0]      = qFP16_Add(x->x[0], tmp3);
    Cals_FpUpdate(x);

    return x->x[0];
}

qFP16_t Cals_FpIntegralSi(Cals_Fpstate_t *x, const qFP16_t s, const qFP16_t dt) {
    // x->x[0] += ((s + (x->x[1] << 2) + x->x[2]) * dt) / 6u;
    qFP16_t tmp1 = qFP16_Add(s, qFP16_Mul(x->x[1], qFP16_Constant(4)));
    qFP16_t tmp2 = qFP16_Add(tmp1, x->x[2]);
    qFP16_t tmp3 = qFP16_Mul(tmp2, dt);
    qFP16_t tmp4 = qFP16_Div(tmp3, qFP16_Constant(6));
    x->x[0]      = qFP16_Add(x->x[0], tmp4);
    Cals_FpUpdate(x);

    return x->x[0];
}

qFP16_t Cals_FpDerivative(Cals_Fpstate_t *x, const qFP16_t s, const qFP16_t dt) {
    x->x[0] = qFP16_Div(qFP16_Sub(s, x->x[1]), dt);
    Cals_FpUpdate(x);

    return x->x[0];
}



#if 0

// 解析积分函数，用于比较
int16_t AnalyticalIntegral(int16_t a, int16_t b, int16_t dt, int16_t t) {
    // 对于 f(t) = at + b，积分 F(t) = 0.5 * a * t^2 + b * t
    return (a * t * t) / 2 + b * t;
}

// 测试函数
void TestIntegralMethods(int16_t a, int16_t b, int16_t dt) {
    Cals_state_t state = { 0 };
    int16_t      t     = 0, i;    // 初始时间
    int16_t      s;               // 当前步的值
    int16_t      integralRe, integralTr, integralSi, integralAnalytical;

    printf("Time\tRe\tTr\tSi\tAnalytical\n");

    // 测试一系列时间步长
    for (i = 0; i < 10; i++) {
        s                  = a * t + b;    // 计算当前时间的函数值
        integralRe         = Cals_IntegralRe(&state, s, dt);
        integralTr         = Cals_IntegralTr(&state, s, dt);
        integralSi         = Cals_IntegralSi(&state, s, dt);
        integralAnalytical = AnalyticalIntegral(a, b, dt, t);

        printf("%d\t%d\t%d\t%d\t%d\n", t, integralRe, integralTr, integralSi, integralAnalytical);

        t += dt;    // 增加时间
    }
}

void test_Cals(void) {
    int16_t a  = 2;    // 假设的线性函数斜率
    int16_t b  = 1;    // 假设的线性函数截距
    int16_t dt = 1;    // 时间步长

    TestIntegralMethods(a, b, dt);
}


#endif


#if 0
Cals_Fpstate_t stateRe = { 0 };
Cals_Fpstate_t stateTr = { 0 };
Cals_Fpstate_t stateSi = { 0 };

// 解析积分函数，用于比较
qFP16_t AnalyticalIntegral(qFP16_t a, qFP16_t b, qFP16_t dt, qFP16_t t) {
    // 对于 f(t) = at + b，积分 F(t) = 0.5 * a * t^2 + b * t
    return qFP16_Add(qFP16_Mul(qFP16_Div(a, qFP16_Constant(2)), qFP16_Mul(t, t)), qFP16_Mul(b, t));
}

// 测试函数
void TestIntegralMethods(qFP16_t a, qFP16_t b, qFP16_t dt) {

    qFP16_t t = qFP16_Constant(0);    // 初始时间
    qFP16_t s;                        // 当前步的值
    qFP16_t integralRe, integralTr, integralSi, integralAnalytical;
    int     i;
    printf("Time\tRe\tTr\tSi\tAnalytical\n");

    // 测试一系列时间步长
    for (i = 0; i < 10; i++) {
        s                  = qFP16_Add(qFP16_Mul(a, t), b);    // 计算当前时间的函数值
        integralRe         = Cals_FpIntegralRe(&stateRe, s, dt);
        integralTr         = Cals_FpIntegralTr(&stateTr, s, dt);
        integralSi         = Cals_FpIntegralSi(&stateSi, s, dt);
        integralAnalytical = AnalyticalIntegral(a, b, dt, t);

        printf("%ld\t%ld\t%ld\t%ld\t%ld\n", t, integralRe, integralTr, integralSi, integralAnalytical);

        t = qFP16_Add(t, dt);    // 增加时间
    }
}

void test_Cals(void) {
    qFP16_t a  = qFP16_Constant(2);        // 假设的线性函数斜率
    qFP16_t b  = qFP16_Constant(1);        // 假设的线性函数截距
    qFP16_t dt = qFP16_Constant(0.01f);    // 时间步长

    TestIntegralMethods(a, b, dt);
}


#endif